/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2022-09-16     chn13935838605       the first version
 */

#include "System.h"
void set_motor_direction(motor_t motor,motor_dir_t dir);

/**
  * @brief  电机驱动函数
  * @param  电机PWM
  * @retval
  */
void MotorOutput(int Motor3_Pwm,int Motor4_Pwm,int Motor5_Pwm,int Motor8_Pwm)
{
        if (Motor3_Pwm >= 0)
        {
            set_motor_direction(MOTOR3,MOTOR_F);
        }
        else
        {
            Motor3_Pwm = -Motor3_Pwm;
            set_motor_direction(MOTOR3,MOTOR_B);
        }
        Motor3_Pwm = (Motor3_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor3_Pwm;

        if (Motor4_Pwm >= 0)
        {
            set_motor_direction(MOTOR4,MOTOR_F);
        }
        else
        {
            Motor4_Pwm = -Motor4_Pwm;
            set_motor_direction(MOTOR4,MOTOR_B);
        }
        Motor4_Pwm = (Motor4_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor4_Pwm;


        if (Motor5_Pwm >= 0)
        {
            set_motor_direction(MOTOR5,MOTOR_F);
        }
        else
        {
            Motor5_Pwm = -Motor5_Pwm;
            set_motor_direction(MOTOR5,MOTOR_B);
        }
        Motor5_Pwm = (Motor5_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor5_Pwm;


        if (Motor8_Pwm >= 0)
        {
            set_motor_direction(MOTOR8,MOTOR_F);
        }
        else
        {
            Motor8_Pwm = -Motor8_Pwm;
            set_motor_direction(MOTOR8,MOTOR_B);
        }
        Motor8_Pwm = (Motor8_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor8_Pwm;

        SET_MOTOR3_COMPAER(Motor3_Pwm);
        SET_MOTOR4_COMPAER(Motor4_Pwm);
        SET_MOTOR5_COMPAER(Motor5_Pwm);
        SET_MOTOR8_COMPAER(Motor8_Pwm);
 }
#if 0
/**
  * @brief  设置电机方向
  * @param  电机，方向
  * @retval
  */
void set_motor_speed(motor_t motor,uint16_t v)
{
  v = (v > PWM_PERIOD_COUNT) ? PWM_PERIOD_COUNT : v;     // 上限处理
  uint32_t dutyfactor = v;
  switch (motor)
  {
  case MOTOR3:
    SET_MOTOR3_COMPAER(dutyfactor);
    break;
  case MOTOR4:
    SET_MOTOR4_COMPAER(dutyfactor);
    break;
  case MOTOR5:
    SET_MOTOR5_COMPAER(dutyfactor);
    break;
  case MOTOR8:
    SET_MOTOR8_COMPAER(dutyfactor);
    break;
  }
}
#endif
/**
  * @brief  设置电机方向
  * @param  电机，方向
  * @retval
  */
void set_motor_direction(motor_t motor,motor_dir_t dir)   
{ 
	switch (motor) {
    case MOTOR3:
            if (dir==MOTOR_F) {
                MOTOR3_IN1_SET;
                MOTOR3_IN2_RESET;
            }else {
                MOTOR3_IN1_RESET;
                MOTOR3_IN2_SET;
            }
        break;
    case MOTOR5:
            if (dir==MOTOR_F) {
                MOTOR5_IN1_SET;
                MOTOR5_IN2_RESET;
            }else {
                MOTOR5_IN1_RESET;
                MOTOR5_IN2_SET;
            }
        break;
    case MOTOR4:
            if (dir==MOTOR_F) {
                MOTOR4_IN1_SET;
                MOTOR4_IN2_RESET;
            }else {
                MOTOR4_IN1_RESET;
                MOTOR4_IN2_SET;
            }
        break;
    case MOTOR8:
            if (dir==MOTOR_F) {
                MOTOR8_IN1_SET;
                MOTOR8_IN2_RESET;
            }else {
                MOTOR8_IN1_RESET;
                MOTOR8_IN2_SET;
            }
        break;
  }
}

/**
  * @brief  使能电机
  * @param
  * @retval
  */
void set_motor_enable(void)
{
      MOTOR3_ENABLE();
      MOTOR5_ENABLE();
      MOTOR4_ENABLE();
      MOTOR8_ENABLE();
}

/**
  * @brief  失能电机
  * @param
  * @retval
  */
void set_motor_disable(void)
{
    MOTOR3_DISABLE();
    MOTOR5_DISABLE();
    MOTOR4_DISABLE();
    MOTOR8_DISABLE();
}
